#!/usr/bin/env python3
# 测试场景：
# 起飞 悬停 降落
import rospy
import time
from std_msgs.msg import String
from std_msgs.msg import Int32
from mavproxy_ros1.msg import FlightCmd
from mavproxy_tool import MavproxyTool

def stardard_test():
    # 起来之后先停10s等待
    time.sleep(10)

    # 发送TAKEOFF指令
    command_pub.publish(tool.get_input_cmd("flight_takeoff"))
    time.sleep(10)

    # 发送悬停指令
    command_pub.publish(tool.get_input_cmd("flight_hover"))
    time.sleep(10)

    # 发送降落指令
    command_pub.publish(tool.get_input_cmd("flight_land"))
    time.sleep(10)

    # 发送TAKEOFF指令
    command_pub.publish(tool.get_input_cmd("flight_takeoff"))
    time.sleep(10)

    # 发送向前飞行指令
    command_pub.publish(tool.get_input_cmd("flight_forward 5"))
    time.sleep(10)

    # 发送向坐指令
    command_pub.publish(tool.get_input_cmd("flight_left 5"))
    time.sleep(10)

    # 发送降落指令
    command_pub.publish(tool.get_input_cmd("flight_land"))
    time.sleep(10)

    pass
  
if __name__ == '__main__':  
    try:
        rospy.init_node('flight_cmd_parser', anonymous=True)
        tool = MavproxyTool()
        command_pub = rospy.Publisher('/mavproxy_ros1/input_cmd', String, queue_size=10)
        stardard_test()
    except rospy.ROSInterruptException:
        pass
